/****************************************************************************
 *
 * (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
 *
 * QGroundControl is licensed according to the terms in the file
 * COPYING.md in the root of the source code directory.
 *
 ****************************************************************************/

#include "SingletonTest.h"
#include "QGCLoggingCategory.h"
#include "QGCToolbox.h"
#include "QGCApplication.h"
#include "SettingsManager.h"
#include "PositionManager.h"
#include <QTime>
#include <QDateTime>
#include <QLocale>
#include <QQuaternion>

#include <Eigen/Eigen>



#include <QDebug>

SingletonTest::SingletonTest():_toolbox                      (qgcApp()->toolbox())
{
    connect(&timer, &QTimer::timeout, this, &SingletonTest::send_mavlink);
    timer.setInterval(500);
    timer.start();
_mavlink = _toolbox->mavlinkProtocol();
connect(_mavlink, &MAVLinkProtocol::messageReceived,        this, &SingletonTest::_mavlinkMessageReceived);
}
SingletonTest::~SingletonTest()
{
}

void SingletonTest::send_mavlink()
{
auto *manager = qgcApp()->toolbox()->multiVehicleManager();
_vehiclelist=manager->vehicles();

if(_vehiclelist && _vehiclelist->objectList()->count()>0)
{

    for(int i=0;i<_vehiclelist->objectList()->count();i++)
    {
        QObject* obj =  _vehiclelist->get(i);
        Vehicle* vehicle0=static_cast<Vehicle*> (obj);
//        if (vehicle0->id()==1)
//        {
            WeakLinkInterfacePtr weakLink = vehicle0->vehicleLinkManager()->primaryLink();
            if (!weakLink.expired()) {
                SharedLinkInterfacePtr sharedLink = weakLink.lock();

                if (!sharedLink) {
                    qCDebug(VehicleLog) << "_handlePing: primary link gone!";
                    return;
                }
                //            auto protocol = qgcApp()->toolbox()->mavlinkProtocol();
                auto priority_link =sharedLink;
                mavlink_message_t msg;
                mavlink_msg_gps_raw_int_pack(vehicle0->id(),
                                             vehicle0->id(),
                                              &msg,
                                             1,
                                             1,
                                             _test1.toLong(),
                                             1,
                                             1,
                                             1,
                                             1,
                                             1,
                                             1,
                                             1,
                                             1,
                                             1,
                                             1,
                                             1,
                                             1,
                                             1);

//                mavlink_msg_adsb_vehicle_pack(vehicle0->id(),
//                                              vehicle0->id(),
//                                              &msg,
//                                              12345,
//                                              _test1.toLong(),
//                                              1,
//                                             1,
//                                              1,
//                                              1,
//                                              1,
//                                              1,
//                                              "1",
//                                              1,
//                                              1,
//                                              1,
//                                              1);
                    vehicle0->sendMessageOnLinkThreadSafe(sharedLink.get(), msg);
                }
//            }
        }
    }

}
void SingletonTest::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t message)
{
switch (message.msgid) {
case MAVLINK_MSG_ID_GPS_RAW_INT:
    _handleGpsRawInt(message);
    break;
case MAVLINK_MSG_ID_ADSB_VEHICLE:
    _handleADSBVehicle(message);
    break;
case MAVLINK_MSG_ID_SENSOR_MESSAGE:
    _handlesensor(message);
    break;
}

}
void SingletonTest::_handlesensor(mavlink_message_t& message)
{
mavlink_sensor_message_t sensor;
mavlink_msg_sensor_message_decode(&message, &sensor);
_test1=QString::number(sensor.unuse1);
emit test1Changed();
}
void SingletonTest::_handleGpsRawInt(mavlink_message_t& message)
{
mavlink_gps_raw_int_t gpsRawInt;
mavlink_msg_gps_raw_int_decode(&message, &gpsRawInt);
//_test1=QString::number(gpsRawInt.lat/ (double)1E7);
//emit test1Changed();
}
void SingletonTest::_handleADSBVehicle(mavlink_message_t& message)
{
mavlink_adsb_vehicle_t adsbVehicleMsg;
mavlink_msg_adsb_vehicle_decode(&message, &adsbVehicleMsg);
if(adsbVehicleMsg.ICAO_address==12345)
{
    _isboat=true;
}
else
{
    _isboat=false;
}
    emit test1Changed();
}
void SingletonTest::setTest1(QString   test1)
{
    _test1=test1;
    emit test1Changed();
}

void SingletonTest::setIsboat(bool   isboat)
{
    _isboat=isboat;
    emit isboatChanged();
}

